#region File information

/**
 * File: HullSide.cs
 * Date: 29-03-2008
 * Auth: Gerard v/d Lei
 * 
 * Desc: Contains the HullSide class which describes a single side of a convex hull.
 *       Methods should only be called from ConvexHull
 */

#endregion

using Microsoft.Xna.Framework;
using System.Collections.Generic;

namespace Gremlin.Collision
{
    /// <summary>
    /// One convex side of a hull, all points (vertices) lie on a single plane
    /// described by the normal and d, such that ax+by+cz+d=0, where n=[a,b,c]^
    /// Point are sorted so that they lie in a circle around the side
    /// </summary>
    class HullSide
    {
        #region Attributes
        private const float SMALL_FLOAT = 0.001f;
        private Vector3 _normal;
        private float _d;
        private List<int> _indices;
        private List<Vector3> _vertices;
        #endregion

        #region Constructors
        /// <summary>
        /// Constructs the side from information that is provided by CollisionProcessor through
        /// ConvexHull
        /// </summary>
        /// <param name="hullSide">The primitive data to construct this side from</param>
        /// <param name="vertices">The vertices to which the indices in hullSide refer to</param>
        public HullSide(Dictionary<string, object> hullSide, ref List<Vector3> vertices)
        {
            _vertices = vertices;
            _indices = (List<int>)hullSide["indices"];
            _normal = (Vector3)hullSide["normal"];
            _d = (float)hullSide["d"];
        }

        /// <summary>
        /// Constructs a side from using the indices of another HullSide
        /// is used to create a translated lightweight copy of this side
        /// </summary>
        /// <param name="reference">Original hullside before translation</param>
        /// <param name="normal">New normal</param>
        /// <param name="d">New D</param>
        /// <param name="vertices">List of vertices to which the indices point</param>
        public HullSide(ref HullSide reference, Vector3 normal, float d, ref List<Vector3> vertices)
        {
            _indices = reference._indices;
            _normal = normal;
            _d = d;
            _vertices = vertices;
        }
        #endregion

        #region Accessors
        /// <summary>
        /// Indices that point to vertices of a ConvexHull
        /// </summary>
        public List<int> Indices
        {
            get { return _indices; }
            set { _indices = value; }
        }

        /// <summary>
        /// Normal of this side
        /// </summary>
        public Vector3 Normal
        {
            get { return _normal; }
            set { _normal = value; }
        }

        /// <summary>
        /// D of the plane on which this side lies
        /// </summary>
        public float D
        {
            get { return _d; }
            set { _d = value; }
        }
        #endregion

        /// <summary>
        /// Checks in O(n) complexitiy if point is in side.
        /// Seems to work and be more efficient then other algorithms. Be wary
        /// </summary>
        /// <param name="p">Point to check</param>
        /// <returns>Where or not the point lies inside the side</returns>
        public bool PointInSide(Vector3 p)
        {
            int i;
            Vector3 dir, edge, disc;
            float side;
            //Checks for each edge on which side the point lies
            //If only once point is on wrong side, return false
            for (i = 0; i < _indices.Count-1; i++)
            {
                dir = p-_vertices[_indices[i]];
                edge = _vertices[_indices[i + 1]] - _vertices[_indices[i]];
                disc = Vector3.Cross(edge, dir);
                side = Vector3.Dot(disc, _normal);
                if (side >= 0)
                    return false;
            }
            //This is the last edge to check, similar to all the previous once
            i = _indices.Count - 1;
            dir = p - _vertices[_indices[i]];
            edge = _vertices[_indices[0]] - _vertices[_indices[i]];
            disc = Vector3.Cross(edge, dir);
            side = Vector3.Dot(disc, _normal);
            if (side >= 0)
                return false;
            //The point was on the right side of all edges, thus return true
            return true;
        }

        /// <summary>
        /// Calculates where a ray intersects with this side.
        /// </summary>
        /// <param name="origin">Origin of the ray</param>
        /// <param name="delta">Direction of the ray</param>
        /// <param name="point">Point where ray intersects</param>
        /// <returns>True when intersection is found, false otherwise</returns>
        public bool RayIntersection(Vector3 origin, Vector3 delta, out Vector3 point)
        {
            //Intersect ray with plane and check if point is in side
            if (CollisionMath.RayPlaneIntersection(origin, delta, _normal, _vertices[_indices[0]], out point))
                if (PointInSide(point))
                    return true;
            return false;
        }

        /// <summary>
        /// Checks in O(n) complexitiy if point is in side. Also checks if this point 
        /// lies close to the edge of a side.
        /// Seems to work and be more efficient then other algorithms.
        /// </summary>
        /// <param name="p">Point to check</param>
        /// <returns>Whether or not the point lies inside the side</returns>
        public bool PointInSideClamp(Vector3 p, out bool onEdge)
        {
            onEdge = false;
            int i;
            Vector3 dir, edge, disc;
            float side;
            //Checks for each edge on which side the point lies
            //If only once point is on wrong side, return false
            //Adds additional check, if point is very close to edge
            //its considered to lie inside the side
            for (i = 0; i < _indices.Count - 1; i++)
            {
                dir = p - _vertices[_indices[i]];
                edge = _vertices[_indices[i + 1]] - _vertices[_indices[i]];
                disc = Vector3.Cross(edge, dir);
                if (disc.LengthSquared() < SMALL_FLOAT)
                {
                    onEdge = true;
                    continue;
                }
                side = Vector3.Dot(disc, _normal);
                if (side >= 0)
                    return false;
            }
            //Check for last side, similar to all the other sides
            i = _indices.Count - 1;
            dir = p - _vertices[_indices[i]];
            edge = _vertices[_indices[0]] - _vertices[_indices[i]];
            disc = Vector3.Cross(edge, dir);
            if (disc.LengthSquared() < SMALL_FLOAT)
            {
                onEdge = true;
                return true;
            }
            side = Vector3.Dot(disc, _normal);
            if (side >= 0)
                return false;
            //If point is on right side of all edges return true
            return true;
        }

        /// <summary>
        /// Calculates where a ray intersects with this side. Also checks if this lies close
        /// to the edge.
        /// </summary>
        /// <param name="origin">Origin of the ray</param>
        /// <param name="delta">Direction of the ray</param>
        /// <param name="point">Point where ray intersects</param>
        /// <returns>True when intersection is found, false otherwise</returns>
        public bool RayIntersectionClamp(Vector3 origin, Vector3 delta, out Vector3 point, out bool onEdge)
        {
            //Intersect ray with plane and check if it is in side
            if (CollisionMath.RayPlaneIntersection(origin, delta, _normal, _vertices[_indices[0]], out point))
                if (PointInSideClamp(point, out onEdge))
                    return true;
            onEdge = false;
            return false;
        }

        /// <summary>
        /// Calculates the two points where the border of this side, intersects a plane
        /// </summary>
        /// <param name="normal">Normal of the plane</param>
        /// <param name="d">D value of the plane</param>
        /// <param name="point">The two points of intersection</param>
        /// <returns>True when there is an intersection, otherwise false</returns>
        public bool PlaneIntersection(Vector3 normal, float d, out Vector3[] point)
        {
            //Determine which points lie on which side
            //On flip, do intersection RayPlaneIntersection
            point = new Vector3[2];
            int count = 0;
            int index1, index0 = _indices[_indices.Count - 1];
            //Check for each set of two point if they lie on same side of the plane or if a flip happened
            int side1, side0 = CollisionMath.DistancePointToPlane(normal, d, _vertices[index0]) < 0 ? -1 : 1;
            for (int cIndex = 0; cIndex < _indices.Count; cIndex++)
            {
                index1 = _indices[cIndex];
                side1 = CollisionMath.DistancePointToPlane(normal, d, _vertices[index1]) < 0 ? -1 : 1;
                if (side0 * side1 < 0) //Flip happened
                {
                    //This means that the edge intersects the plane, find the point where this happens
                    CollisionMath.RayPlaneIntersection(_vertices[index0], _vertices[index1] - _vertices[index0], normal, -d * normal, out point[count]);
                    count++;
                }
                //Can only cross plane twice
                if (count == 2)
                    return true;
                side0 = side1;
                index0 = index1;
            }
            //If there aren't to intersection points, side did not cross plane
            return false;
        }

        /// <summary>
        /// Calculates the two point where the borders of two sides intersect
        /// </summary>
        /// <param name="side">Side to check against</param>
        /// <param name="point">The points of intersection</param>
        /// <returns>True when intersection happens</returns>
        public bool HullSideIntersection(HullSide side, out Vector3[] point)
        {
            //Find plane intersection of 'this' side with 'side's plane
            bool res = PlaneIntersection(side._normal, side._d, out point);
            if (!res)
                return false;
            //Check if both points lie in side
            bool onEdge;
            bool p0 = side.PointInSideClamp(point[0], out onEdge);
            bool p1 = side.PointInSideClamp(point[1], out onEdge);
            //We already found both intersections points return true
            if (p0 && p1)
                return true;
            //Not a single intersections, means no HullSide intersection
            if (!(p0 || p1))
                return false;
            //We know there is an intersection, now find the section intersection point

            //One point is in side, other not, so this point lies on line of 'side'
            Vector3[] point2;
            res = side.PlaneIntersection(_normal, _d, out point2);

            //Point closest to discarded point of first check is de correct one
            int discarded = p0 ? 1 : 0;
            float l0 = (point[discarded] - point2[0]).LengthSquared();
            float l1 = (point[discarded] - point2[1]).LengthSquared();
            point[discarded] = (l0 < l1) ? point2[0] : point2[1];

            return true;
        }
    }
}
